#ifndef SRC_UTILS_CLOUD_HPP_
#define SRC_UTILS_CLOUD_HPP_

#include <algorithm>
#include <cstdint>
#include <list>
#include <memory>
#include <stdexcept>
#include <vector>

#include "n_lidar_obj/util/richpoint.hpp"
#include "n_lidar_obj/util/useful_typedefs.h"

/**
 * @brief Class for point container.
 *
 */
class PointContainer
{
public:
  PointContainer() = default; // 默认构造函数
  inline bool IsEmpty() { return _points.empty(); }
  inline std::list<size_t> &points() { return _points; }
  inline const std::list<size_t> &points() const { return _points; }

private:
  std::list<size_t> _points;
};

/**
 * @brief      A class that stores a vector of RichPoints
 * @details    A utility class  for storing points. If PCL is available has ways
 *             of converting to and from pcl. Also knows how to generate a
 *             projection from its points and can be generated from an image.
 */
class Cloud
{
  // class PointContainer; // todo:如何使用
  // some useful usings

public:
  using PointColumn = std::vector<PointContainer>; // 一列点
  using PointMatrix =
      std::vector<PointColumn>; // vector < vector<list> > 点的Matrix
  using Ptr = shared_ptr<Cloud>;
  using ConstPtr = shared_ptr<const Cloud>;

  // 组合类的构造函数
  // 默认构造函数
  Cloud(){
      //_data = PointMatrix(32, PointColumn(1801));
      // _data = PointMatrix(1801, PointColumn(32)); //
      // 放到depth_cluster类里面去
  };
  // 组合类的拷贝构造函数
  Cloud(const Cloud &cloud) : _points(cloud._points) {}
  // Cloud(const Cloud& p) {_data = p._data;}
  // explicit Cloud(const Pose& pose) : _pose(pose), _sensor_pose() {}

  ~Cloud() {}

  inline const RichPoint::AlignedVector &points() const { return _points; }

  inline void push_back(const RichPoint &point)
  {
    _points.push_back(point);
  } // todo: 存点的实体
  // inline void push_back(const RichPoint* point) { _points.push_back(point); }
  // // todo: 存点的实体
  inline size_t size() const { return _points.size(); }
  inline bool empty() const { return _points.empty(); }
  inline void clear() { return _points.clear(); }
  inline void reserve(size_t size) { _points.reserve(size); }

  // 重载赋点没有实现
  // inline Cloud &operator=(Cloud::Ptr a)
  // {
  //     // 浅拷贝
  //     // this->_points = a->_points;
  //     // 深拷贝
  //     // memcpy(this->_points, a->_points, a->_points.size());
  //     // return *this;

  //     // 给对象new一个空间 在函数外面已经new过了
  //     for (size_t i = 0; i < a->size(); i++)
  //     {
  //         this->push_back(a->_points[i]);
  //     }
  //     return *this;
  // }

  inline RichPoint &operator[](int idx) { return _points[idx]; }
  inline const RichPoint &operator[](int idx) const { return _points[idx]; }
  // inline PointContainer& at( size_t row,  size_t col){
  //    return _data[col][row]; //todo:先列再行
  //}
  inline RichPoint &at(int idx) { return _points[idx]; }
  inline const RichPoint &at(int idx) const { return _points[idx]; }

  inline void Resize(size_t new_size) { _points.resize(new_size); }

// PCL specific part
#if PCL_FOUND
  typename pcl::PointCloud<pcl::PointXYZL>::Ptr ToPcl() const;

  template <class PointT>
  static Cloud::Ptr FromPcl(const pcl::PointCloud<PointT> &pcl_cloud)
  {
    Cloud cloud;
    for (const auto &pcl_point : pcl_cloud)
    {
      RichPoint point(pcl_point.x, pcl_point.y, pcl_point.z);
      cloud.push_back(point);
    }
    return make_shared<Cloud>(cloud);
  }
#endif // PCL_FOUND
  PointContainer _pointcontainer;
  RichPoint::AlignedVector _points;
  // PointMatrix _data; // vector < vector<list> > // 放到depth_cluster类

  // Pose _pose;
  // Pose _sensor_pose;
private:
};

#endif // SRC_UTILS_CLOUD_HPP_